/*
 * T_Splitsing2.c
 *
 * Created: 24-5-2013 11:09:43
 *  Author: Niels Markhorst
 */ 


#include <avr/io.h>
#include "T-Splitsing2.h"
#include "globals.h"




void TSplitsing2(void)
{
	
	
	
	int senArray1[3];
	uint8_t andByte;
	int staArray1[7][4] =
	{
	{1,0,0,2},
	{0,1,0,4},
	{0,0,1,6},
	{1,1,1,4},
	{1,1,0,2},
	{0,1,1,6},
	{1,0,1,2}
	};


	int tellerEen, tellerTwee, tellerDrie, tellerVier=0, tellerVijf=0, tellerZes=0, tellerZeven=0, tellerAcht=0, tellerNegen=0, tellerTien=0, tellerElf=0;

	for(int x=0;x<3;x++)
	{
		stoplichten2[x]=lichtArray[presentState2-1][x];
	}




	if (PINA & (1<<3) )
	{
		senArray1[0] = 1;
	}
	else
	{
		senArray1[0]= 0;
	}

	if (PINA & (1<<4))
	{
		senArray1[1] = 1;
	}
	else
	{
		senArray1[1]=0;
	}

	if (PINA & (1<<5))
	{
		senArray1[2] = 1;
	}
	else
	{
		senArray1[2]=0;
	}




	if(presentState2==1)
	{
		
		
		
		tellerVier=1;
		
		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray1[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray1[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==6)
						{
							tellerElf=tellerVier;
						}
						
						
					}
				}
			}
		}
	}

	else if(presentState2==2)
	{
		tellerVier=2;
		int staArray2[7][4] =
		{
		{1,0,1,3},
		{0,1,1,3},
		{0,0,0,3},
		{0,0,1,3},
		{0,1,0,3},
		{1,1,0,3},
		{1,1,1,3},
		};
		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray2[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray2[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerElf=tellerVier;
						}
					}
				}
			}
		}
	}

	else if(presentState2==3)
	{
		
		
		int staArray3[8][4] =
		{
		{0,0,0,1},
		{1,0,0,1},
		{1,1,0,1},
		{1,1,1,1},
		{0,1,1,1},
		{0,0,1,1},
		{0,1,0,1},
		{1,0,1,1},
		};
		for(tellerEen=0; tellerEen<8; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray3[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray3[tellerEen][3];
					}
				}
			}
		}
		
		staArray1[3][3] = 4;
		staArray1[4][3] = 4;
		staArray1[6][3] = 6;
		
	}

	else if(presentState2==4)
	{
		tellerVier=4;
		int staArray4[7][4] =
		{
		{0,1,1,5},
		{1,0,1,5},
		{0,0,0,5},
		{0,0,1,5},
		{1,0,0,5},
		{1,1,0,5},
		{1,1,1,5},
		};
		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray4[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray4[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerElf=tellerVier;
						}
					}
				}
			}
		}
	}

	else if(presentState2==5)
	{
		
		
		int staArray5[8][4] =
		{
		{0,0,0,1},
		{1,0,0,1},
		{1,1,0,1},
		{1,1,1,1},
		{0,1,1,1},
		{0,0,1,1},
		{0,1,0,1},
		{1,0,1,1},
		};
		for(tellerEen=0; tellerEen<8; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray5[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray5[tellerEen][3];
					}
				}
			}
		}
		
		staArray1[3][3] = 6;
		staArray1[4][3] = 2;
		staArray1[5][3] = 6;
		
	}

	else if(presentState2==6)
	{
		tellerVier=6;
		int staArray6[7][4] =
		{
		{0,1,1,7},
		{1,1,0,7},
		{0,0,0,7},
		{0,1,0,7},
		{1,0,0,7},
		{1,0,1,7},
		{1,1,1,7}
		};
		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray6[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray6[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerElf=tellerVier;
						}
					}
				}
			}
		}
	}

	else if(presentState2==7)
	{
		
		
		int staArray7[8][4] =
		{
		{0,0,0,1},
		{1,0,0,1},
		{1,1,0,1},
		{1,1,1,1},
		{0,1,1,1},
		{0,0,1,1},
		{0,1,0,1},
		{1,0,1,1},
		};
		for(tellerEen=0; tellerEen<8; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray7[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray7[tellerEen][3];
					}
				}
			}
		}
		
		staArray1[3][3] = 2;
		staArray1[5][3] = 4;
		staArray1[6][3] = 2;
		
	}


	if(tellerVier!=0 && tellerVijf==0 && tellerZes==0 && tellerZeven==0 && tellerAcht==0)
	{
		presentState2=tellerVier;
	}
	if(tellerVijf!=0)
	{
		presentState2=tellerVijf;
	}
	else if(tellerZes!=0)
	{
		presentState2=tellerZes;
	}
	else if(tellerZeven!=0)
	{
		presentState2=tellerZeven;
	}
	else if(tellerAcht!=0)
	{
		presentState2=tellerAcht;
	}
	else if(tellerNegen!=0)
	{
		presentState2=tellerNegen;
	}
	else if(tellerTien!=0)
	{
		presentState2=tellerTien;
	}
	else if(tellerElf!=0)
	{
		presentState2=tellerElf;
	}




	if (stoplichten2[0] == 1)
	{
		orByteC |= 0b10000000;
	}
	if (stoplichten2[0] == 2)
	{
		orByteC |= 0b01000000;
	}
	if (stoplichten2[0] == 3)
	{
		orByteC |= 0b00100000;
	}
	
	//------------------------------------------------------------------------------
	if (stoplichten2[1] == 1)
	{
		orByteD |=  0b00100000;
	}
	if (stoplichten2[1] == 2)
	{
		orByteD |=  0b01000000;
	}
	if (stoplichten2[1] == 3)
	{
		orByteD |=  0b10000000;
	}
	
	//-------------------------------------------------------------------------------
	if (stoplichten2[2] == 1)
	{
		orByteD |=  0b00000100;
	}
	if (stoplichten2[2] == 2)
	{
		orByteD |=  0b00001000;
	}
	if (stoplichten2[2] == 3)
	{
		orByteD |=  0b00010000;
	}


if(orByteC == 0b00110000 || orByteC == 0b00100100 || orByteD == 0b10000100 || orByteD== 0b00110000)
{
	vertraging2 = 4;
}
else if(orByteC == 0b01010000 || orByteC == 0b01000100 || orByteD == 0b00101000 || orByteD == 0b01000100)
{
	vertraging2 = 2;
}
else
{
	vertraging2 = 1;
}


}